#include "car.h"

void CarBase::InitCar(const double& pos_x, const double& pos_y, const double& heading,
    const double& length, const double& width) {
    car_length = length;
    car_width = width;
    heading_theta = heading;

    R0 = std::hypot(car_width / 2.0, car_length / 2.0);
    theta0 = std::atan(car_length / car_width);

    pmid = std::make_unique<Point>(pos_x, pos_y); // 中心点
    plf = std::make_unique<Point>(pmid->x - car_width / 2.0, pmid->y + car_length / 2.0, PI - theta0, R0);
    prf = std::make_unique<Point>(pmid->x + car_width / 2.0, pmid->y + car_length / 2.0, theta0, R0);
    plr = std::make_unique<Point>(pmid->x - car_width / 2.0, pmid->y - car_length / 2.0, PI + theta0, R0);
    prr = std::make_unique<Point>(pmid->x + car_width / 2.0, pmid->y - car_length/ 2.0, -theta0, R0);
    
    //旋转各点
    plf->PointTurn(*pmid, heading_theta);
    prf->PointTurn(*pmid, heading_theta);
    plr->PointTurn(*pmid, heading_theta);
    prr->PointTurn(*pmid, heading_theta);
    
    // 更新前后轴中点
    UpdatePmidf();
    UpdatePmidr();
}

// 更新前轴中点
void CarBase::UpdatePmidf() {
    double x = (plf->x + prf->x) / 2.0;
    double y = (plf->y + prf->y) / 2.0;
    // pmidf是个独有指针，如果指针不为空则更改其值即可，否则在进行创建
    // 独有指针智能指向一块内存，如果已经存在了，不能在进行创建指向其他的内存地址了
    if (pmidf) {
        pmidf->x = x;
        pmidf->y = y;
    } else {
        pmidf = std::make_unique<Point>(x, y);
    }
}

// 更新后轴中点
void CarBase::UpdatePmidr() {
    double x = (plr->x + prr->x) / 2.0;
    double y = (plr->y + prr->y) / 2.0;
    if (pmidr) {
        pmidr->x = x;
        pmidr->y = y;
    } else {
        pmidr = std::make_unique<Point>(x, y);
    }
}

// 更新几何中点
void CarBase::UpdatePmid() {
    double x = (plf->x + prr->x) / 2.0;
    double y = (plf->y + prr->y) / 2.0;
    if (pmid) {
        pmid->x = x;
        pmid->y = y;
    } else {
        pmid = std::make_unique<Point>(x, y);
    }
}

// 绘制车辆矩形
void CarBase::ShowCar(const COLORREF& color) {
    // 设置线的样式（实线）
    setlinestyle(PS_SOLID, 4);
    // 设置线条颜色
    setlinecolor(color);
    // 以顺时针方向将四个点之间用线连起来
    line(plf->x, plf->y, prf->x, prf->y);
    line(prf->x, prf->y, prr->x, prr->y);
    line(prr->x, prr->y, plr->x, plr->y);
    line(plr->x, plr->y, plf->x, plf->y);
}

// 绘制轨迹信息
void CarBase::ShowCircle() {
    // 设置线的样式（虚线）
    setlinestyle(PS_DOT, 2);
    // 设置线条颜色
    setlinecolor(MAGENTA);
    // 绘制车辆四个顶点的轨迹
    circle(p_center->x, p_center->y, Rof);
    circle(p_center->x, p_center->y, Ror);
    circle(p_center->x, p_center->y, Rif);
    circle(p_center->x, p_center->y, Rir);
}

// 打印输出信息
void CarBase::CoutInfo() {
    std::cout << "pmidr->x = " << pmidr->x << ", pmidr->y = " << pmidr->y << ", pmidr->Rp = " << pmidr->Rp << ", pmidr->thetaP = " << pmidr->thetaP << std::endl;
    std::cout << "pmidf->x = " << pmidf->x << ", pmidf->y = " << pmidf->y << ", pmidf->Rp = " << pmidf->Rp << ", pmidf->thetaP = " << pmidf->thetaP << std::endl;
    std::cout << "pmid->x = " << pmid->x << ", pmid->y = " << pmid->y << ", pmid->Rp = " << pmid->Rp << ", pmid->thetaP = " << pmid->thetaP << std::endl;
    std::cout << "plf->x = " << plf->x << ", plf->y = " << plf->y << ", plf->Rp = " << plf->Rp << ", plf->thetaP = " << plf->thetaP << std::endl; 
    std::cout << "prf->x = " << prf->x << ", prf->y = " << prf->y << ", prf->Rp = " << prf->Rp << ", prf->thetaP = " << prf->thetaP << std::endl; 
    std::cout << "plr->x = " << plr->x << ", plr->y = " << plr->y << ", plr->Rp = " << plr->Rp << ", plr->thetaP = " << plr->thetaP << std::endl; 
    std::cout << "prr->x = " << prr->x << ", prr->y = " << prr->y << ", prr->Rp = " << prr->Rp << ", prr->thetaP = " << prr->thetaP << std::endl; 
    std::cout << "speed = " << speed << ", speed_x = " << speed_x << ", speed_y =  " << speed_y << ", a = " << a << ", delta_theta = " << delta_theta / PI << ", delta_theta_rot = " << delta_theta_rot / PI << ", heading_theta = " << heading_theta / PI << ", Gear = " << Gear << std::endl;
}


// 单帧直行
void CarBase::MoveStraightStep() {
    plf->PointMove(speed_x, speed_y);
    prf->PointMove(speed_x, speed_y);
    plr->PointMove(speed_x, speed_y);
    prr->PointMove(speed_x, speed_y);
    pmid->PointMove(speed_x, speed_y);
    pmidf->PointMove(speed_x, speed_y);
    pmidr->PointMove(speed_x, speed_y);
}

// 单帧转向
void CarBase::CarTurnStep() {
    pmidr->PointTurn(*p_center, delta_theta);
    plf->PointTurn(*p_center, delta_theta);
    plr->PointTurn(*p_center, delta_theta);
    prf->PointTurn(*p_center, delta_theta);
    prr->PointTurn(*p_center, delta_theta);
    heading_theta += delta_theta;
}

// 更新四个半径
void CarBase::UpdateRinRout(const double& R) {
	Ror = R + car_width / 2.0;
	Rir = R - car_width / 2.0;
	Rof = hypot(Ror, car_length); //sqrt(pow(Ror, 2.0) + pow(car_length, 2.0));
	Rif = hypot(Rir, car_length); //sqrt(pow(Rir, 2.0) + pow(car_length, 2.0));
}

// 更新转向信息
void CarBase::UpdateTurnInfo(const int& turn_state, const double& R) {
    double x = 0.0;
    double y = 0.0;
    // 更新四个半径
    UpdateRinRout(R);

    if (turn_state == TurnDirection::TurnRight) {
        // 右转
		x = pmidr->x + R * cos(heading_theta);
		y = pmidr->y - R * sin(heading_theta);
        // 更新5个点的角度和半径
		pmidr->thetaP = heading_theta + PI;
		pmidr->Rp = R;

        // 因为和后轴转向中心共线,所以转向角相等
		plr->thetaP = pmidr->thetaP;
		plr->Rp = Ror;

		prr->thetaP = pmidr->thetaP;
		prr->Rp = Rir;

		plf->thetaP = pmidr->thetaP - atan(car_length / Ror);
		plf->Rp = Rof;

		prf->thetaP = pmidr->thetaP - atan(car_length / Rir);
		prf->Rp = Rif;

    } else {
        // 左转
        x = pmid->x - R * cos(heading_theta);
        y = pmid->y + R * sin(heading_theta);
        // 更新5个点的角度和半径
		pmidr->thetaP = heading_theta;
		pmidr->Rp = R;

        // 因为和后轴转向中心共线,所以转向角相等
		plr->thetaP = pmidr->thetaP;
		plr->Rp = Rir;

		prr->thetaP = pmidr->thetaP;
		prr->Rp = Ror;

		plf->thetaP = pmidr->thetaP + atan(car_length / Rir);
		plf->Rp = Rif;

		prf->thetaP = pmidr->thetaP + atan(car_length / Ror);
		prf->Rp = Rof;
    }
    // 更新转向中心
    if (p_center) {
        p_center->x = x;
        p_center->y = y;
    } else {
        p_center = std::make_unique<Point>(x, y);
    }
}

void CarBase::UpdateXYva() {
    speed_x = speed * sin(heading_theta);
    speed_y = speed * cos(heading_theta);
    a_x = a * sin(heading_theta);
    a_y = a * cos(heading_theta);
    std::cout <<"speed_x = " << speed_x << " speed_y = " << speed_y << " a_x = " << a_x << " a_y = " << a_y << std::endl;
}

// 更新车辆直行信息
void CarBase::UpdateStraightInfo() {
    // 更新中心点信息
    UpdatePmid();
    UpdatePmidf();
    UpdatePmidr();
    // 更新速度和加速度
    UpdateXYva();

    p_center.reset(); // 指针指向空（直行没有转向，不需要转向中心，释放之前指向的内存）
    // 更新转向半径信息
    Rof = 0.0; // 外前轮半径
    Rif = 0.0; // 内前轮半径
    Rir = 0.0; // 内后轮半径
    Ror = 0.0;

    // 更新极坐标下转向相关的信息,三个中心点 + 自车四个顶点
    pmid->thetaP = 0.0;
    pmid->Rp = 0.0;
    pmidr->thetaP = 0.0;
    pmidr->Rp = 0.0;
    pmidf->thetaP = 0.0;
    pmidf->Rp = 0.0;

    plf->thetaP = 0.0;
    plf->Rp = 0.0;
    prf->thetaP = 0.0;
    prf->Rp = 0.0;
    plr->thetaP = 0.0;
    plr->Rp = 0.0;
    prr->thetaP = 0.0;
    prr->Rp = 0.0;

    // 更新转向相关的角速度
    delta_theta = 0.0;
    delta_theta_rot = 0.0;
}

/************一般车辆***************/
CarNormal::CarNormal(const double& pos_x, const double& pos_y, const double& heading,
    const double& length, const double& width) {
    InitCar(pos_x, pos_y, heading, length, width);
}